//
// Created by Pulsar on 2020/5/16.
//

#include <rc_task/rcCVTask.h>
#include <rc_log/slog.hpp>
#include <rc_task/rcTaskVariable.h>

#ifdef WITH_LIBREALSENCE2

#include <librealsense2/rs.hpp>

#endif
#define WIDTH 640
#define HEIGHT 480
#define FPS 30


namespace rccore {
    namespace task {
        CvTask::CvTask(std::shared_ptr<common::Context> pcontext) : BaseTask(pcontext) {

        }

#ifdef WITH_LIBREALSENCE2

        void CvTask::Run() {
            m_thread = new std::thread([&]() {
                slog::info << "视觉设备启动中" << slog::endl;
                rs2::context ctx;

                slog::info << "当前SDK版本 - " << RS2_API_VERSION_STR << slog::endl;
                int res_number = ctx.query_devices().size();

                slog::info << "当前深度摄像头连接数 " << res_number << " RealSense devices connected" << slog::endl;
                if (res_number == 0) {
                    slog::info << "深度摄像头不存在，启用默认摄像头" << slog::endl;
                    cv::VideoCapture cap(pcontext->pconfig->pconfigInfo->CAMERA_INDEX);
//                    cap.set(cv::CAP_PROP_FPS, pcontext->pconfig->pconfigInfo->CAMERA_FPS);
                    if (not cap.isOpened()) {
                        slog::err << "摄像头启动失败" << slog::endl;
                        exit(-1);
                    }
                    slog::info << "摄像头打开成功:" << pcontext->pconfig->pconfigInfo->CAMERA_INDEX << slog::endl;
                    task::task_variable::TASK_NUM += 1;
                    while (not m_isStop) {
                        std::unique_lock<std::mutex> m_locker(m_mutex);
                        m_cv.wait(m_locker, [this] { return !m_isPause; });
                        cv::Mat frame;
                        cap.read(frame);
                        if (frame.empty())break;
                        cv::resize(frame, frame, cv::Size(pcontext->pconfig->pconfigInfo->RESIZE_WIDTH,
                                                          pcontext->pconfig->pconfigInfo->RESIZE_HEIGHT));
                        //调试窗口打开
                        if (pcontext->pconfig->pconfigInfo->DEBUG_WINDOW) {
                            cv::imshow("origin", frame);
                            cv::waitKey(1);
                        }
                        cv::Mat clone_frame = frame.clone();
                        pcontext->pmessage_server->pimageMessage->push_message(clone_frame);
                        m_locker.unlock();
                        std::this_thread::sleep_for(std::chrono::microseconds(50));
                    }
                    cap.release();
                    slog::warn << __FUNCTION__ << " 摄像头任务退出" << slog::endl;
                    task::task_variable::TASK_NUM -= 1;
                } else {
                    try {
                        rs2::pipeline pipe;
                        //创建一个以非默认配置的配置用来配置管道
                        rs2::config cfg;
                        cfg.enable_stream(RS2_STREAM_COLOR, WIDTH, HEIGHT, RS2_FORMAT_BGR8, FPS);//向配置添加所需的流
                        cfg.enable_stream(RS2_STREAM_DEPTH, WIDTH, HEIGHT, RS2_FORMAT_Z16, FPS);
                        pipe.start(cfg);
                        task::task_variable::TASK_NUM += 1;
                        while (!m_isStop) {
                            rs2::frameset frames = pipe.wait_for_frames();
                            rs2::depth_frame depth = frames.get_depth_frame();
                            rs2::frame color = frames.get_color_frame();

                            cv::Mat depth_image(cv::Size(WIDTH, HEIGHT), CV_16U, (void *) depth.get_data(),
                                                cv::Mat::AUTO_STEP);
                            cv::Mat color_image(cv::Size(WIDTH, HEIGHT), CV_8UC3, (void *) color.get_data(),
                                                cv::Mat::AUTO_STEP);
                        }
                        pipe.stop();
                        task::task_variable::TASK_NUM -= 1;
                    } catch (rs2::error &e) {
                        slog::err << "深度摄像头启动失败 " << slog::endl;
                        slog::err << e.what() << slog::endl;
                    }
                }
            });
        }

        CvTask::~CvTask() {

        }

#else
        void CvTask::Run() {

        }
#endif
    }
}